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l.U INTRODUCTION 

A forward sequential digital filter, such as a Kalman filter, estimates a state 
vector from measurements that were made preceding a timepoint in question. 

This type of filter is suitable for processing measurement data in real time 
because data following current time have not yet been received. However, it is 
clear that the best estimate of a state vector at a given time will be obtained 
by using data from both sides of the timepoint in question. Thus, optimal 
postfactum state vector evaluation requires the use of a ’'smoothing" filter. 


2.0 DEFINITIONS 


= true state vector at time t-i 


= measurement vector at time tj. is zero-mean, 
tlmewise-uncorrelated measurement noise 


T 

Qj = E(aj,£j) = measurement noise covariance matrix 
Xj/k = estimate of xj based on jr|c* 

= estimate of £(xj) based on j-1 measurements 




= measurement partials matrix at time 


^3 


£j/k = 2Lj/k “ 2£j - state estimation error 


T 

^j/k = ®(^j/k»S.j/k) stafc^ error covariance matrix 

2ij+1 = s j = forward integration equation for true state vector; 

Sj is zero-mean, timewise-uncorrelated state 
noise. 


T 

Sj = E(sj,sj) = state noise covariance matrix 
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‘ = forward integration equation for the estimated state 

vector 


ij/N = n) - (backward integration equation for the estimated 

~ * (based on N measurements) state vector 


s. forward state transition matrxx 

* = xj/j 


-1 

" backward state transition matrix 


♦jtl/j 


ax 

ax 


N = total number of timepoints at which data are available 


3.0 THE FORWARD ESTIMATE 

Let the forward estimation equations be given by 


! 

1 Ij/j-1 = £(xj/j-l) 

1 

! 

! 

(3.1) 

* « 

' Wj(Zj - Ij/j.i) 

! 

! 

_! 

(3.2) 

where Wj is the measurement weighting 

or forward gain matrix. 



The error equation associated with equation (3.2) is obtained as follows. 

Ij = £(xj) + aj = fi(xj/j -1 - ej/j.',) + 3j 

Assuming that is small, a first-order Taylor series expansion can be made. 

« 

Ij = - Pj 


2 


* iyj-1 

o equation (3.2) can be written 


2«j + £J/j * 


>r 


Thus 


Dj = I - WjPj 

£j/j = ^yyi ■*■ 

T T 

Cyj = Dj Dj + Wj Qj Wj 


The true state vector* is propagated ahead by 


Xj+1 = X(2Cj) + sj = f(Xj/j - £j/j) 
For small estimation errors 

Xj +1 = f(xj/j) - *j+1/j jEJ/J + Sj 


But 


£(iyj) = 2£j+i/j 


and 


Xj+1 = Xj+1/j - £yi/j 
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so the propagation equations are 


^ 

1 XJ+1/J = f(Xj/j) j 

(3.6) 

I ^ 

* £j+1/j = "^j+1/j ‘ j 

I 1 

(3.7) 

j Cj+1/j = '^j^-i/j Cj/j *j+vj + Sj 1 

(3.8) 


The Kalman filter equations are now completed by minimizing the quadratic form 
zT Cj/j z with respect to Wj for all nonzero vectors z. This will cause 
Xj/j to”be a minimum variance estimate. Let 


a s Cj/j z (a scalar) 

Using equation 3.5 

^ = zT(-Pj Cj/j„i(I - Wj Pj)T - (I - Wj Pj)Cj/j.i Pj 
+ Qj Wj + Wj Qj) z 

= (-Pj Cj/j-ld - Wj Pj)T + Qj W^) z 

+ zT (- (I - Wj Pj)Cj/j.i Pj f Wj Qj) z 
This will equal zero if 

(I - Wj Pj) Cj/j_T Pj = Wj Qj 


or if 
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! Wj * 


I 

Cj/J-1 Pj (Pj Cj/j-1 Pj + Qj)"^ I 


(3.10) 


which is the optimal forward weighting matrix. Note that the predicted resid- 
ual variance is 



(3.11) 


This information, free of charge, can be used to edit, say, 6o residuals. 
Substituting equations (3*9) and (3.10) into equation (3.5) yields 


Cj/j = Pj Cj/j-1 = Cj/j-1 Pj 


( 3 . 12 ) 


Or, using equations (3.3) and (3.10) for the optimum Wj, 



(3.13) 


This is the equation for Cj/j when Wj is optimum. 

4.0 THE BACKWARD (SMOOTriED) ESTIMATE 

For is 1, 2, N-1 let the backward, or smoothed, estimate of the state 

be given by 
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J£N-i/N * 

A A 

iN-i/N * ISN-i/N + Bn-l^XN-l/N-i " XN-i/N> 


(4.1) 

(4.2) 


where la the backward gain matrix to be derived. 

For the purpoae of deriving error equatlona, equation (4.2) will be modified in 
the following manner. From equation (4.1), it la aeen that 


iN-i/N = X"^^ai-i+1/N-i + (xN-i+l/N " 2N-ii-l/N-i)^ 

For small state estimation errora 

XN-i/N = M-i/N-i + %.d/N-i+l (xN-i+1/N " XN-i+l/N-l) (4.3) 

Substitution into equation (4.2) yields 

A 

2£N-i/N = 2£N-i/N + %-i(xN-i+1/N-i " iN-i-fl/N) (^-4) 


or 


SN-i/N = 2N-1/N-1 - C*’N-i/N-i+1 - °N-i) 
(2N-i-»-1/N-i - -^N-i+l/N^ 


where the backward gain matrix is 


! ! 

! GN_i n BN_i ‘i'N-i/N-i-f-1 * 
I ! 


(4.5) 


(4.6) 
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The error equation aasooiated with xn- 1/N obtained from equation (<l.5) 

£N-i/N * £N-i/N-i ♦ (♦n-I/N-I+I " Cn-l><lN-i+1/N “ £N-l+1/N-i> 

But from equation (3*7) i it is seen that 

£N-i/N-i * ♦N-l/N-it-I^SN-i+l/N-i + SN-i) 

Thus» the state error propagation equation la 

I ^ ^ ! 

* £N-1/N = <*N-l/N-i+1 - ON-i) £N-i-*-l/N * 

* ON-i £N-i+VN-l * 

i I 

* + ♦n-I/N- 1+1 5^>-i • 

In order to evaluate 

Cn- 1/N - E(eu_i/N JEN-I/n) 
expressions for 

T 

E(£N-i+1/N Bl-i+1/N-i3 
and 

T 

e(£N- 1+1/N SN-i) 


are needed. To obtain these, define the matrix (i = 2, 3, N-1) by 
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« Hn- 1 = ((♦N-i+1/N-i+2 - %-Ul) HN-I-H 

+ Gn-i+i) • 

* ♦N-i+2/N-i+1 %-i+1 


1 and 


1 3 D|j K I “ W{^ P(j for 1=1 



Using the state error propagation equations (3*^)» (3*7) and (4.7) t it can 
be shown that 

T 

EtjlN-i+1/N lN-i+1/N-l5 = %-i CN-i+1/N-i 
and 

kUn- 1+I/N iN-i) = '%-i Sn- 1 
It can now easily be shown that 


^N-i/N = ‘*’N-i/N-i+l(CN-i4-1/N + ~ ^N-i) %-i “ %-i %-i) ‘^N-i/N-i+1 

1 T 

- GM-iCcN-i+1/N + (I - Hw-i) Sn-i - %-i+1/N-i Hn-.O %-i/N-i+l 

T T 

- %-i/N-i+ltCN.i4i/N + SN-i(I “ Hn_i) - CN_i+i/N„j_ 10^-1 

T T 

+ GN_i(CN_i+i/N + CN-i+1/N-i (i ‘ Hn-i) - CN-i+i/N-ijON-i 


(4.11) 


The backward, or smoothing, toquations for optimal or suboptimal W's and G's 
are summarized in table I. 


(4.9) 

(4.10) 
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The matrix G|j_^ will now be chosen so as to minimize the quadratic form, 
z^C(].i/}| z, for all z> be a minimum variance estimate of 

The value of (which accomplishes this) is easily seen to be 


i 


{ 


T 

°N-i = ♦N-i/N-i+1 (CN-i+1/N + Sn-i(I - H^.i) - Cn-i+i/N-i) 

T 

X (CN-i+1/N + CN-i+i/N-i(I - HN-i) - Cn_i+i/N-i)"^ 

(4.12) 


It is also easily seen that 
<J'n-i/N-i+l - GN-i = 


T 

^N-i/N-i+1 (Cn-i+1/Noi " SN-i)(I - Hn.i) 

, T 

X lGN-i+1/N + CN_i+i/N-i (I - Hn.i) 

- Hn_i C^.i+i/N-i)"”' 


(4.13) 


Substituting the optimum value of Gjj_i, given by equation (4.12), into equation 
(4,11) yields 


T T 

^N-i/N = *N-i/N-i+1 (CN-i+1/N + U - HN_i)SN_i - S^.i Hu_i)^N-i/N-i+1 

T T 

- GN-i(CN-i+i/N + (I - H^.i) Sn- 1 - Cu.i+i/ij.i %-i) ^N-i/N-i+l 

(4.14) 


From the equations using the optimal forward gain matrix (equations (3.9), 
(3.10), and (3.12)) it can be shown, using proof by induction, that 


i 


-1 

%-i = CN_i+i/N CN-i+1/N-i 


(4.15) 


The previous equations may be combined in several ways to give the optimal 
smoothing equations. Three versions are presented here in tables II, III, and 
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fcoraafcimes, as a oheoic on the functioning of the filter, it la desired to see 
how well residual statisticst agree with their predicted covariance matrix. 

Table V shows three forma of residuals with their associated covariance 
matrices. 

Suppose that a smoothed estimate of the state Is required at the fixed epic 
time, t^. Using version 3 of the smoothing equations (table IV), it can bo 
shown that the equations in table VI result. Note that these equations operate 
forward in time in conjunction with the normal forward (Kalman) filter 
equations. 

The question arises as to which version of the smoothing equations is best. 

For problems with nonlinear dynamics, the author prefers version 1. Version 2 
is simpler and faster, but it has a residual with a time tag that is not at the 
correction time. An additional linearity assumption has been made in this ver- 
sion, namely 


XN-i+1/N-l - XN-i+1/N - ♦N-i+1/N-i^iN-i/N-i “ *N-i/N) 


(i<.l6) 


Version 3 also suffers from this defect. Version 3 is relatively simple and 
needs no backward integrator. However, take the limiting case where the t-e 
noise covariance is aero. Versions 1 and 2 correctly integrate the state back- 
ward by 


iN-i/N = I’HxN-i-fl/N) 


(4.17) 


But, version 3 integrates backward by 


XN-i/N = 2N-i/N-i + ^N-i/N-i+l(*N-i+1/N “ 2(N-.l+1/N-i) (4.18) 


which is not as accurate as equation (4.17). In the case of linear dynamics, 


>yj-i/N = %-i/N-i+l XN-i+1/N (4.19) 


and all versions give the same answers except for computer roundoff error. In 
the case of linear dynamics, the author prefers version 3 of the smoothing 
equations. 

A final case needs to be mentioned. The forward Kalman filter frequently pro- 
cesses successive scalar measurements at a single timepoint rather than the 
measurement vector. That is, a four-element measurement vector may be pro- 
cessed as four scalar measurements, all at the same timepoint. This is equiva- 
lent to setting $ = I and S = 0 between measurements. For the smoothing 
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equations, save only the results after the last scalar measurement has been 
processed. The results at this time will be equivalent to processing all mea- 
surements at once as a vector. 
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TABLE I.- SMOOTHING EQUATIONS FOR OPTIMAL OR SUBOPTIMAL 
Gn -1 and Dn_i = 1 - W(j.iPN-i 


Initialize 


«N-i = Dn - I - WnPn 


Cn-I+I.A'J “ Cn/n 


iLV-i+1/N = XN/N 


For i = 1. 2. .... N-1 


m-i = I"^<XN-i+1/N) 


‘^N-i^l/N-i = 


^N-i/N-i+1 = *N-i+1/N-i 


X = 2y<-i/N-i 


2N-i = iN-i/N + %-i ‘^N-i+1/N-i(iN-i/N-i " XN-I/n) 

T ^ T 

CN-i/N - '^N-i/N-i+1^^^N-i+1/N -t (I - HN-i)SN-i - S^.j. 

T T 

- OtJ-i(cN.i+yN + (I “ HN_i)SN-i - CN-i+1/N-i HN-i)^-i/N-i+1 

- ‘**N-i/N-i+1^CN-i+i/N + SN_i(l - Hn_i) - CN-i4.i/N-i)GN-i 

+ GN-i''CN-i+1/N + %-i+1/N-i(I " %-i) " ^^N-i CN-i^1/N-i-'%-i 

HN_i_1 = ^(*N-i/N-i+1 - Gn-i) %-i + %-i^'*’N-i+1/N-i *^N-i 




( 
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TABLE II.- VERSION 1 OF THE OPTIMAL SMOOTHING EQUATIONS 
FOR OPTIMAL FORWARD (KALMAN) EQUATIONS 


( 


Initialize 


CN-i+ 1 /N = Cn/N 


For i = 1 . 2 N -1 


XN-i+ 1 /N = i 9 «/N 


2 fW-i/N = £“’(*N-i+ 1 /N) 


3 f 

*^N-i+ 1 /N-i = -- 
3 x 


♦n-I/N- 1+1 = 


♦N-i+ 1 /N-i 


X = XN-i/N-i 


%-i = *N-i/N-i +1 %-i ^N-i/N-i+l (Cn-I/N-I 

T 

+ %-i/N-i +1 Sn_i *N-i/N-i+l)“' 


XN-i/N = XN-i/N + %-i(xN-i/N-i “ xN- 1 /n) 


CN-i/N = CN-i/N-i - (I - BN-i)'*’N-i/N-i+l(CN-i+l/N-i 

T 

- CN_i+i/N) ^N-i/N-i+lU - Bn_i)T 

T 

= (I - BN.i)*N_i/N-i+1 CN_i+i/N '**N-i/N-i+l(I - ^-i)"^ 

+ Bfj>i CN_i/N-i 


A m 

= (I - BN_i)'I'N_i/N-i +1 Cu-i+i/N ^N-i/N-i+l <1 ~ 


+ (I - Btj>i) *N-i/N-i +1 Sn -1 <I>N-i/N-i +1 


NOTE: I - Bfj_i = Cfj.i/N_i (CN_i/N_i + "J^N-i/N-i+l S(j-i %-i/N-i+l)‘^ 
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TABLE in.- VERSION 2 OP THE OPTIMAL SMOOTHING EQUATIONS 
FOR OPTIMAL FORWARD (KALMAN) EQUATIONS 


Initialize 


Cn-1+I/N * ^N/N 


2N-1+1/N * XN/N 


For 1 g 1 . 2. .... N-1 

XN-i/N = X“‘'(xN-i+1/N) 


♦N-i+1/N-i 


31 

im 

Bx 


X = 2SN-1/N-1 


-1 

♦N-i/N-i+1 * ♦N-1^1/N-i 


-1 

GN-i = ♦N-i/N-i+1 %-! Cn-I+I/N-I 

2£N-i/N = XN-i/N + GN-i(xN-i+1/N-i " 2SN-1+1/N) 

Cn-1/N = Cn-I/N- 1 - (♦N-i/N-i+1 " Qn-iHCn-I+I/N-I 
- Cn-1+i/n) (*N-i/N-i+1 “ 

= (♦N-i/N-i+1 - GN-i)CN-i+1/N(*N-i/N-i+1 " Gn-i)’’^ 

T 

+ (*N-i/N-i+1 " %-i) Sn- 1 *N-i/N-i+1 
NOTE: G(j_i = Bn_i *N-i/N-i+1 
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TABLE IV.- VERSION 3 OF THE OPTIMAL SMOOTHING EQUATIONS 
FOR OPTIMAL FORWARD (KALMAN) EQUATIONS 


Initialize 

CN-i+1/N “ Cn/n 


2SN-i+1/N = 2N/N 


For 1=1.2 N-1 


♦N-i+1/N-i 



XN-i/N-i 


-1 

♦N-i/N-i+1 = ♦N-i+1/N-i 


T -1 

Rn-I = CN_i/N_i *N-i+1/N-i Cs-i+l/N-i 

= - Sn- 1 Cijli+l/N-i) (preferred form) 

XN-i/N = XN-i/N-i + ^N-i^XN-i+l/N " 2£N-i+1/N-i) 


T 

CN-i/N = - RN-i(CN-i+1/N-i " CN-i+1/N)RN-i 

T T 

= RN-1 Cn-I^VN RN-1 + Rn- 1 %-i ♦N-i/N-i+1 


NOTE: Rn_i = (I - Bn-i) 4«N-i/N-i+1 = tN-i/N-i+l " °N-l 


80FM32 
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TABLE V.- SMOOTHING RESIDUAL COVARIANCE MATRICES 

The covariance matrix of the realdual jyt.i/N-i " 2N-1/N 

T 

♦N-i/N-i+l(CN-i+l/N-i - Cn-1+I/n) ♦N-I/N-1+1 
The covariance matrix of the realdual XN.j:/|f.j[ - XN-l/N 

T 

(I - %-i)*N-i/N-i+l(CN-i+i/N-i - CN_i+i/N) ♦N-I/N-1+1 “ ^N-i)^ 

= (♦N-i/N-i+1 - Gn-iHCn-i+vN-I “ C^.i+VN^^N-l/N-i+l " Gn-i)’^ 
The covariance matrix of the residual 2^1-1+ 1/N-i “ i£N-i+1/N 

CN-i+1/N-i - %-i+1/N 
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TABLE VI.- OPTIMAL SMOOTHING EQUATIONS* FOR 
A FIXED EPIC TIME, 


At tlwa t 8 tw initialize 

^n/n+j-l * ®n/n ln/n+J-1 * i£n/n ^ 

For J =1, 2, 3. ... 

3f -1 

'*’n+j/n+j-l * ~ ^n+J-1/n+J ~ “^n+J/n+J-l 

3x 

i * 2Ln+J-1/n+j-1 
T -1 

•^n+d-1 = Cn4.j_i/n+j-i ®n4-j/n+j-1 ^n+j/n+J-1 

= ♦n+d-1/n+J (I - Sn+j-1 Cn|j/n+j-i) (preferred form) 

^n+j = Kn+j_i Rn+j-1 

2£n/n+j = 2in/n+j-1 + ^n+j (2£n+j/n+j ” *n+j/n+J-0 

T 

®n/n+d = ^n/n+j-1 “ *^n+j ^^n+j/n+j-1 " ^n+j/n+j^^n+j 


®Note that the above equations operate forward in time in conjunction with 
the normal forward (Kalman) filter equations. 
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5.0 THE BACKWARD EQUATIONS OF MOTION 

Many (perhapa moat) Kalman flltera contain exponentially correlated random 
atate varlablea in the atate vector. In thia caae, backward integration ia 
more Involved than Juat aetting ^T negative. A aimple example will illua- 
trate the problem. 

Let the acceleration of a point be ax(t). Let ax be an exponentially 
correlated random variable, conatant over each integration atep. Poaition, ve- 
locity and acceleration atatea alfe integrated forward in the Kalman filter by 


Xi+1 s Xi + x^ At + ax,i At2/2 (5*1) 

Xi+1 = XI + ax,i (5.2) 

ax, 1+1 = a ax,i (5.3) 

where 

a = exp(-AT/Tajj) (0 < a < 1) (5.M) 


Solving the above equations for x^, xi and ax^i yields the backward integra- 
tion equations 


Xi = Xi+1 - ii^i AT + 2 ax i+1 At2/2 

a ' 


(5.5) 


. . 1 

Xi = Xi+1 - - 3xji+1 


AT 


(5.6) 


1 

ax,i = " ax i+i 
a 


(5.7) 

i 


where At = ti+i - ti > 0. To integrate backwards, the rules are this: for 

the nonexponentially correlated states 
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a. Exchange the i and i+1 subsorlpta 

b. Set AT s -AT 

* 

\ 

0 . Replace ax with -ax 

a 

For the exponentially correlated state, integrate backward by using 
1 

^x,i • "®x,i+1 


The above example was for linear dynamical equations where the inverse solution 
was easily obtained. However, for complex nonlinear dynamics, the rules are 
precisely the same. For example, consider that the acceleration of a point is 
given by the nonlinear differential equation 


y = ax(t) - k(t)x2 


(5.9) 


where ax(t) and k(t) are exponentially correlated random variables, 
constant across the integration step. Note that k(t) symbolizes a time vari- 
able drag coefficient.' Note that 


X = -2kxx = -2kx(ax " kx^) 


(5.10) 


The forward equations of motion are easily seen to be 

. >2 At2 . ,2 ^t3 

Xi+1 = Xi xi at (ax,i - ki .ti) -j- - 2ki xi(ax,i - ki x,t ) 

(5.11) 

• 2 • #2 

Xi +1 = Xi + (ax,i - ki xl) at - 2ki Xi (ax,i - ki X0 At2/2 (5.12) 

3x,i+1 ~ ^1 %,i (5.13) 

ki+1 = S2 ki (5.14) 
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Where 


ai * exp (-/ff/ Tax) (5.15) 

82 * exp("AT/Ti() (5.16) 

To obtain the backward equations of motion ^ merely apply the previously 
defined simple rules. 


H 


= Xi-M - H*] *x,i^l " ^ xLlj^^''2 


+ 2 


^ ki+1 Xi^.i( ^ ax,i+1 - ^ ki+1 Xi+^\ AT3/6 

32 \ 31 S2 y 


/ 1 1 .2 \ 
Xi = xi +1 -I - ax,i+l ki +1 Xi +1 / 

\ ai ’ 82 / 




1 • M 1 

- 2— ki+1 Xi+il — 3x,i+i - — ki+1 

32 \ai »2 


•2 \ 
Xi+1 1 


AT2/2 


(5.17) 


(5.18) 


1 

®x,i = 3x i+i 

8i 


(5.19) 


1 

ki = — ki+1 
82 


(5.20) 


where AT above is a positive quantity. 

Finally a word about the inverse of the state transition matrix. ♦N-i+i/N-i 
is almost always of the form 


soPMsa 


I 


♦N-i+1/N-t * 



♦l|_ 


(5.^7) 



where *4 ie frequently diagonal • The inverse is then easily seen to be 


♦N-i/N-i+1 « 




L 0 



♦2 




(5.28) 


6.0 EXAMPLE 1 

Very precise altitude measurements will be made of a falling sphere in order 
to determine the density of the atmosphere. The sphere will be dropped 
from an altitude of h * 11 000 meters. Altitude measurements will be made 
every 0.1 second. The altitude measurement random noise will have a standard 
deviation of 


' oq s 0.1 meter ( 6 . 1 ) 

f 

'■fS 

c I 

The equation of motion is given by 


X 


-ac + “ Cn 
» 2 



( 6 . 2 ) 


where the acceleration due to gravity is 


Bg = 9.8 ra/seo^ 


For h < 11 000 meters the atmosphere density is assumed to be given by 

p = (1 + 6 ) PO (1 - 0.0065h/288.15)‘**2559 (6.4) 

where 


Po = 1.2250 kg/m 3 

-4 


( 6 . 5 ) 


i 

i 
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Equation (6.<1) with fi * 0 la the equation for the 1962 Standard Atnoaphere. 
1006 ia the peroentage deviation from the atandard. 6 wiU be a atate 
variable in the filter. 

Let 


Cd 


A 

- X 0.01 

W 


m2 

kg 


( 6 . 6 ) 


Then equation (6.2) beoomea 


I . . ' 

J X X -9.8 + 0.006125(1 - 0.0065x/288.15)'*-2559(i ♦ fi)x2 | (6.7) 

I 


From equation (6.7) (with x x 0) the ground (x x h s 0) impact velocity ia aeen 
to be about m/aec x 89 niph. At an altitude of x x h = 11 000 metera, the 
ateady state velocity ia about 73.^ m/aeo x 16^1 mph. The aphere takea about 
207 seconds to reach the ground (with 6x0). 

For th real world, 6 will be chosen to be 


f 



( 6 . 8 ) 


Thus the real-worlc density will deviate from the 1962 Standard Atmosphere by 
±S percent. For the filter world, 6 will be taken as an exponentially correl- 
ated random variable whose standard deviation is 0.035 (3.5 percent) with a 
time constant of 100 seconds. That is, for the filter world 


^i+1 = a 6^ + ofi Hi (6.9) 

a = exp(-AT/T) (6,10) 


T X 100 seconds 


05 = 0.035 
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n it zero-mean, Wmewiae-uncorrelated , state noise with a unit variance. 
The state vector, x, will be 


X s 


X s altitude, h 

• e 

X = h 

<S = scale factor error 


( 6 . 11 ) 


ormSuSrare°Led^‘"‘”“ th, following equation. 


= Xi + Xi At + *x*i At2/2 


Xi+i K Xi + x'i AT 


( 6 . 12 ) 

(6.13) 


6i+1 = a 6i 


(6.110 


where = 0*1 second and x was given by equation (6.7), 
forward state transition matrix are 


Elements of the 


3^i+1 3xi 

$11 = = 1 + — ^j2/2 

3^i 3*i 


(6.15) 


3xi+i 3xi 

•**12 = — J — = AT + -T- AT2/2 

3Xi 3Xi 


( 6 . 16 ) 


ns 


3xi+i 9*1 


36i 86i 


rr- At 2/2 


(6,17) 


‘'21 = 


3xi+i 3*i 


3xi 3xi 


AT 


( 6 . 18 ) 


3x 


‘’22 = 


i+1 


3xi 


3 Xq - 


= 1 + 




At 


(6.19) 
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♦ „ !!i!2 . A 


36i+1 

*31 - -7: = 0 

^ 3xi 


^ 3 ^ 1+1 

f32 = _ = 0 


3^i+1 

^33 ~ “ 


where 


3xi 

r — ^ 0 (x error contribution small) 

3xi 


• • 

3xi 

= 0.01225 (1 - 0.0065X/288. 15 * * 2559(1 + 6i)xi 

3xi 



0.006125 (1 - 0.0065x/288.15)***2559xj2 


( 6 . 20 ) 


(6.21) 


( 6 . 22 ) 


(6.23) 


(6.24) 


(6.25) 


( 6 . 26 ) 


For both the real world and the filter, a fourth-order Nystrom-Lear integrator was 

used to integrate equation (6.7) to give x and x. In the real world equation 
(6.8) gave 6 as a function of time. In the filter integrator, 6 is taken as 
a constant across the integration step. That is, for the forward filter 
Integrator 
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AT s 0.1 

Integrate ic s f(x, x, Ai) Ai constant 
* a «i 
T = T + AT 

For the backward Integrator in the filter 


At = -0.1 
h - '^i+l/a 

Integrate x = f(x, x, 6^ constant 

T = T -f At 


Note that the backward integration of x s f(Xf x, 6) is not "stable" and 
thus is a good test of the smoothing equations. The reason for the instability 
is that the downward fall of the sphere quickly reaches a "steady-state" condi- 
tion, irrespective of the initial conditions. Because of computer roundoff error, 
the backward integration of the fall will generally yield absurd initial 
conditions. 


From equation (6.9), the state noise covariance matrix is seen to be 



0 0 

0 0 

0 a?(1-a2) 


(6.27) 


where 


2 

CJ6(1-a2) 


0.24475 51634*10-5 


( 6 . 28 ) 


Because measurements of x are made, the measurement partlals matrix is 


P = (l 0 o) 


(6.29) 


The logistics of the computer program are not simple. They are outlined below. 
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a Initialize! T s initial measurement time, Xn+i/n» ®n+1/n» 

Process measurement to give Xn/n> ^n/n* 

Print T, Xn/n, Cn/n, etc. 

If last measurement go to b. 

Compute ^n+l/n* 

Integrate x^j/n ahead to give x^j+i/n* 

Propagate C^/n ahead to give 

Write tape of T, Xn+1/nt ^n/n* Cn^1/n» '^n+1/n- 

T = T + AT. 

GO to a. 

b BACKSPACE TAPE. 

Set Cu^i+i/N = ^n/n a"'* iN-i+l/N = 2Sn/n* 

AT = -AT. 

c Read tape into: T, x^.i/^.i, Bl-i+l/N-i* CN_i^T/N_l, 4»N-i+1/N-i* 

Go through smoothing equations to compute 
Print T, XN_i/N. CN_i/N, etc. 

If (T = Tinitial) STOP. 

BACKSPACE TAPE. 

BACKSPACE TAPE. 

GO TO c. 

Figure 1 shows the actual scale factor, 6, and the Kalman filter solution for 
6. The Kalman filter predicted standard deviation for the error in 6 was 
about 0.0065, down from 0.035. Figure 2 shows the version 1 smoothing filter so 
lution for the scale factor, 6. Its standard deviation was about 0.0027, a fac- 
tor of 2.4 better than the Kalman filter solution. Figure 3 shows a plot of 
the Kalman filter velocity error versus time. Also shown on the plot is the 
predicted 10 velocity error standard deviation, about 0.05 m/sec. Figure 4 
shows the version 1 smoothing plot of velocity error and its predicted standard 
deviation of 0.016 m/seo, a factor of 3-1 improvement. 
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The predicted standard deviation of the position error for the Kalman filter 
was 0.043 meter, for the smoothing filter it was 0.020 meter - a factor of 2.2 
Improvement . 

Version 3 of the smoothing filter equations was also run, and it showed very sim* 
liar results compared to version 1. This was a little surprising because version 
3 is more highly linearized than version 1, and the equations of motion are 
highly nonlinear, in addition to being unstable when integrated backward. 




















80FM32 


7.0 EXAMPLE 2 

This example la for a highly aimpllfied tracking version of a Spaoe Shuttle 
launch. The two-dimenaional tracking geometry la shown in figure 8. 


Y 



Rl-^ 

— ^R2 


Figure 5.- Shuttle tracking geometry. 

The values for the location of the radar stations will be 

XRi = -5000 meters 
XR2 = 5000 meters 

The actual biases adding to the radar measurements will be 

®Api = 12 meters, constant 
®AE1 = 0.15 milliradian, constant 
®AP2 = 12 meters, constant 
®AE2 = ®*15 milliradian, constant 
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The standard deviation of the actual randoa error adding to the Meaureaents is 

‘JqApl * <Jqkp2 » 9 aeter^'i 
cfqAEl * f^qAE2 • 0.15 ailliradian 

The ffleaaurement bias errors are modeled in the Kalman filter as exponentially 
correlated random variables with a time constant of 106 seconds. Their stan- 
dard deviations are 


OBpl = 03p2 = 12 meters 
OBE! - (^BE2 ” 0.15 milliradian 

The standard deviations of the random errors adding to the Kalman filter mea- 
surements are taken to be 


Oqpi = Oqp2 = 9 meters 

OqEl = OqE2 =0.15 milliradian 

They are the same as those used for the real world. 

The true, real-world trajectory is generated by the following equations. 


*T,i+1 = XTI + Xxi AT + x’xi AT2/2 + Xxi AT3/6 (7.1) 

yT,i+1 = yTi + yTi at + yxi AT2/2 + y’xi AT3/6 (7.2) 

^T,i+1 = *xi + xxi AT + xxi AT2/2 (7.3) 

at 

yT,i+1 = m + yii at + yxi AT2/2 (7.1|) 

I ^T,i+1 * xxi + xxi AT (7.5) 

yT,i+i = yii + m at (7.6) 


♦ 
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*1,1+1 * *T1 * 0*^ B/seo3, constant 

yr,i+1 ® yxi ® m/sec3, constant 

Initial values are, at T s O, 

Xf s -1.E6 meters 
XT s 0 
XT = 0 

x’t = 0.1 m/sec3, constant 
VT = 100 000 meters 

XT = 0 
yT = 0 

yj s 0.01 ra/aeo3, constant 
AT, the measurement sample interval, is 
AT s 0.1 second 

At T s 300 seconds, Xf = 30 m/sec2 = 3g, Vt * 3 m/sec^. At this time 
x*T and yT are set to zero because the maximum Shuttle acceleration is 
about 3 g's at staging. 

At T s 300 seconds, the true state is 

XT = -.55E6 meter 

XT = '1500 m/sec 

XT = 30 m/sec^, reset to 0 

yT = 145 000 meters 

• 

yj = 450 m/sec 

yT = 3 m/sec2, reset to 0 


(7.7) 

(7.8) 
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^ Tracking is tepiainatcd at T * 600 aaoonda. At thla tlaa, 

XT * 1.25E6 netera ■ 675 n. ml. 
xj « 9000 b/bcc 
XT * 30 m/aeo^ 

MT ” 325 000 metepa « 175»5 n. ml. 

* a 

VT * 900 n/aeo 


y*T * 3 m/aeo2 




state vector dynamics for the Kalman filter are given by 


’‘i-M s Xi Xi At + xi At2/2 



(7.9) 

yi+1 - Yi * h ^ ‘h 



(7.10) 

Xi+1 = ii + XI At 



(7.11) 

yi+1 = yi + yi ^ 

j 



(7.12) 

•• / 2 

Xi +1 " ^ax’^i axy^ “ *ax 



(7.13) 

•• " n r ^ 

yi+1 = aayyi + °ayVl * aay 

V.I 


(7.1^0 * 

®Pl,i+1 = asPl Bpiji + <%pi \ 

f 2 

'1 - aflPi 

%Pl,i 

(7.15) 

0E1,i+1 = 3BE1 Bgi^i + <^bE1 \ 

1 2 
'1 - SBEI 

^E1,i 

(M6) 
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Bp2,i+1 * «Bp2 Bp2,i 

♦ OBp2 

1 2 
1 - SBpZ 

■ 

nBp2,i 

(7.17) 

BE2,1+1 * “BEZ BE2,1 

+ obezV 

2 

1 - SBBe 

HBE2,i 

(7.18) 


where the ni *re zero-mean, unlt-varianoe , tlmewlae-unoorrelated random 
variables t The terms oontaining the m are the Kalman filter state noise 
terms. Note that in integrating the state ahead, the best estimate of Hi ie 
zero. The "a" values above are given by 


*ax * exp(-PT/Tajj) 

tax * seconds 

(7.19) 

3ay * exp(-AT/tay) 

Tay a 40 seconds 

(7.20) 

aepi a exp(-AT/TBpi) 

tBpi a 108 seconds 

(7.21) 

SBE 1 * exp(-AT/TBEl ) 

tbei a 108 seconds 

(7.22) 

aBp 2 a exp(-AT/TBpz) 

tBp2 * 108 seconds 

(7 23) 

®BE2 * exp(-AT/lBE2) 

Tbb 2 * 106 seconds 

(7.24) 


It is seen that the dynamical equations are linear. That is, the state vector, 
is propagated ahead by 


Xi+1 * *i+1/i J£1 


(7.25) 


where the state transition matrix is given on the next page and its 

Inverse on the page aftnr that. Note the large number of zero elements in 

*i+1/i ^i+1/i* "^his is typical and may be used to speed the matrix 

arithmetic. • 






0 1/bbPI 000 
0 0 ^ 0 
000 1/3BP2 ® 

0 0 0 0 1/ai 
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The range measurement from station 1 Is given by 


Pi = V(x - xri) 2 + y2 -f Bpi (7*26) 

And the elevation angle measurement from station 1 Is 


El = arotan + Bri 

X - XRl 

The partial derivatives are 


3pi 

X - XR1 

3x 

V(x - Xri) 2 + y2 

3pi 

y 

3y 

V(x - xri)2 + y2 

3pi 


■■■ ; 

= 1 

3Bpi 


3Ei 

y 

3x 

(x - Xri) 2 + y2 

3Ei 

X - XRl 

3y 

(x - xri) 2 + y2 

3Ei 


3Bei 

= 1 


Similar equations apply to P2 and Eg 
And finally, the state noise covariance 


(7.27) 


( 7 . 28 ) 


(7.29) 


(7.30) 


(7.31) 


(7.32) 


(7.33) 


matrix is given on the next page. 
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0 Oaxd-aax) 0 0 0 0 0 

2 2^ 

0 0 <Jay<1-aay) 0 0 0 0 

2 2 , 

0 0 0 ®BPld“aBpl^® ® ® 

2 2 ^ 

0 0 0 0 <JBEld-aBEl)0 0 

2,2^ 

0 0 0 0 0 <JBp2d-aBp2)0 


2 2 
0 OBE2d-aBE2/ 


0 


0 


0 


0 


0 
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Figure 6 ahows the magnitude of the velocity error vector veraua time for the 
Kalman (forward) filter. Figure 7 ahowa the aame thing for the amoothing fil- 
ter. Note that ataging ooourred at 300 aeoonda. The amoothing filter reaulta 
are much better (about 3 or 4 timea better) than the Kalman filter reaulta. 
Figurea 8 and 9 ahow the velocity errora aoroaa ataging in finer detail. Again 
it ia aeen that the amoothing filter reaulta are auperlor to thoae of the 
Kalman filter. The amoothing filter haa a much lower maximum error and a 
ahorter tranalent time acroaa ataging. Alao note how amooth the curve la for 
the amoothing filter in figure 9. The amothlng filter ia aptly named beoauae 
Ita output appeara to be very "amooth" oompared to the Kalman filter. 

Figurea 10 and 11 ahow the Kalman filter and amoothing filter aolutlona for the 
X component of acceleration aoroaa ataging. x ia aeen to go from 3g to zero 
at ataging. Again it ia aeen that the amoothing filter aolution la very amooth 
and Improved over that of the Kalman filter. 

The amoothing filter did not improve the poaitlon errora by much, maybe 5 per- 
cent. Also the radar biaa errora in thia example were not well determined by 
either the Kalman filter or the amoothing filter. 








o 


Figure 7.- Magnitude of velocity error vector for smoothing filter. 
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8.0 MATRIX INVERSION ALGORITHMS v 

Th® smoothing equAtions roquir® ®ffioi®nt matrix inv®rsion algorithms. Four 
algorithms ar® pr®s®nt®d h®r®.» Th®s® algorithms ar® v®ry faat and raquir® a 
minimum of storag®. Th® matrix to b® inv®rt®d ia H, an M by N matrix. If 
H * hT th® inv®rsion r®quir®s about m 3/2 additions and multiplioations. If 
H 4 hT than th® invarsion raquiras about n 3 multiplioationa and additions. ^ 

Not® that multiplication of two N by H matrioas raquiraa about N3 multi- < 

plications and additions. Not® that th® algorithm® war® writtan for clarity 
and may b® spaadad up avan mor® by ravisad programing. 


3These algorithms were obtained from William M. Lear, "Kalman Filtering 
Techniques", NASA/JSC Internal Note 78-FM-25, April 1978. 
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8.1 FORTRAN ALGORITHM FOR H > HHBRB H i 
DIMENSION H<N,N) 

H(l,1) « 

IF (N. EQ. 1) RETURN 
NMl * N-1 
5 Jsl, NMl 
1 1*1, J 
H(I,J+1) * 0. 

D(|) 1 K*1, J 

1 Hd.J+l) = H(I,J+1) - H(J+1,K)»H<I,K) 

D(}) 2 K=1, J 

2 H(J+1,J4-1) * H(J+1,J+1) + H(J+1,K)»H(K,J^1) 
H(J+1,J+1) * 1./H(J+1,J+1) 

D4> 3 1=1, J 

3 H(J+1,I) * H(I,J+1)«H(J+1,J.t-1) 

D(j) 4 1=1, J 

D4> 4 K=I, J 

H(I,K) = H(I,K) + H(I,J>M)»H(J+1,K) 

4 H(K,I) = H(I,K) 

D4. 5 1=1, J 

5 H(I,J+1) = H(J+1,I) 

RETURN 
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END 
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8.2 FORTRAN ALGORITHM FOR Q > H‘1 HHBRB H * HT 
DIMENSION G(N,N), H(N,N) 

G(1,l) > 

IF (N. EQ. 1) RETURN 
NM1 * N-1 
Dt 5 NM1 
D4) 1 Isl, J 
G(I,J+1) s 0. 

D4> 1 Ksl, J 

1 G(I,J+1) s G(I,J+1) - H(J+1,K)«G(I,K) 
G(J+1,Jt1) = H(J+1,J+1) 

P-t) 2 Ksl, J 

2 G(Jv1jJ+1) * G(J+1,J+1) + H(J4-1,K)«G(K,J+1) 
G(J+1,J+1) = 1./G(J+1,J+1) 

C'4> ? 1=1, J 

3 G(j-»-';i:) = G(i,j-»-T)»G(j+i,j+i> 

D(J) 1=1, J 

D({) 4 K=I, J 

G(I,K) = G(I,K) + 0(1,J+1)»G(J+1,K) 

4 G(K,I) = G(I,K) 

D<j) 5 1=1, J 

5 G(I,J+1) = G(J+1,I) 

RETURN 

END 
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8.3 FORTRAN ALGORITHM FOR H s H’1 WHERE H 4 H^' 
DIMENSION F(N-1), H(N,N) 

H(1,1) s 1./H(1,1) 

IF (N. EQ. 1) RETURN 
NM1 = N-1 
D(|) 5 Jsl, NM1 
D4) 1 Isl, J 
F(I) = 0. 

D(() 1 Ksl, J 

1 F(I) = P(I) - H(I,K)«H(K,J+1) 

D4) 2 K:^1, J 

2 = H(J+1,J+1) + H(J+1,K)»F(K) 
H(J+1,J+1) = 1./H(J+1,J+1) 

D(() 3 1=1, J 

H(I,J+1) = F(I)«H(J+1,J+1) 

F(I) = 0. 

D(}> 3 K=1, J 

3 F(I) = F(I) - HM+1,K)»H(K,I) 

D(|> 4 1=1, J 

D(j) 4 K=1, J 

4 H(I,K) = H(I,K) + H(I,J+1)»F(K) 

D4» 5 1=1, J 

5 H(J+1,I) = H(J+’l,J+1)«»F(I) 

RETURN 

END 
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8.4 FORTRAN ALGORITHM FOR G c H'1 WHERE H M HT 
DIMENSION G(N,N), H(N,N) 

0(1,1) = 1./H(1,1) 

IF (N. EQ. 1) RETURN 
NM1 = N-1 
D4> 5 J=1, NM1 
D(^ 1 1=1, J 
G(I,N) = 0. 

D4> 1 K=1, J 

1 G(I,N) = G(I,N) - G(I,K)»H(K,J+1) 
G(J+1,J+1) = H(J+1,J+1) 

D4) 2 K=1, J 

2 G(J+1,J+1) s G(J+1,J+1) + H(J+1,K)«G(K,N) 
G(J+1,J+1) = 1./G(J+1,J+1) 

D(J> 3 1=1, J 

G(I,J+1) = G(J+1,J+1)«G(I,N) 

G(N,I) = 0. 

D4) 3 K=1, J 

3 G(N,I) = G(N,I) - H(J+1,K)«G(K,I) 

D(j) 4 1=1, J 

D(|) 4 K=1, J 

4 G(I,K) = G(I,K) + G(I,J+'|)»G(N,K) 

D(t> 5 1=1, J 

5 G(J+1,I) = G(J+1,J+1)»G(N,I) 
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9.0 CONCLUDING REMARKS 


It has been seen that the smoothing filter can considerably Improve accuracy 
over that of a Kalman filter. Factors of improvement for velocity errors of up 
to three or four times have been seen here when position measurements were being 
processed . 

Three versions of the smoothing equations have been presented. Of the three, 
version 1 is theoretically the most accurate but requires the most computer 
time. If the equations of motion are linear, that is, if 


2£i+1 = *i+1/i ii 


then all three versions should give identical results except for computer 
roundoff error. 

The state estimation equations for the smoothing filter appear to be quite sta- 
ble. In example 1, the backward integration of the equations of motion was un- 
stable but, in the presence of the smoothing equations, became stable. Example 
2 showed that the smoothing filter output was Indeed quite smooth when compared 
to the noisy Kalman filter estimates. 
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